// Coordinate follows mavros 

#include <ros/ros.h>

#include <tf/transform_datatypes.h>

#include <message_filters/subscriber.h>
#include <message_filters/synchronizer.h>
#include <message_filters/sync_policies/approximate_time.h>
#include <message_filters/time_synchronizer.h>

#include <geometry_msgs/PoseStamped.h>
#include <geometry_msgs/Pose.h>
#include <geometry_msgs/PointStamped.h>
#include <geometry_msgs/TwistStamped.h>
#include <geometry_msgs/Point32.h>
#include <std_msgs/Float64MultiArray.h>
#include <sensor_msgs/PointCloud2.h>
#include <visualization_msgs/Marker.h>
#include <pcl/common/transforms.h>
#include <pcl/filters/voxel_grid.h>
#include <pcl/point_types.h>
#include <pcl_conversions/pcl_conversions.h>
#include <iostream>
#include <string>
#include <cmath>
#include <algorithm>
#include <tf/transform_broadcaster.h>
#include <time.h>
#include <stdlib.h>
#include <mavros_msgs/State.h> 
#include <random>
#include <nav_msgs/Odometry.h>
#include <control_msgs/JointControllerState.h>
#include <std_msgs/Float64.h>
#include <queue>
#include <fstream>
#include <cstdint>
#include <trajectory_msgs/JointTrajectoryPoint.h>
#include "vied_map_array_improve_5_cluster_velocity_zyx.h"

using namespace message_filters;
using namespace std;

#define PIx2 6.28318
#define PI 3.14159
#define PI_2 1.5708

/**** Parameters to tune, some initialization needs to be changed in main function ****/
VideMap my_map;

const unsigned int MAX_POINT_NUM  = 5000;
float point_clouds[MAX_POINT_NUM*4];

float x_min = -MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float x_max = MAP_LENGTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float y_min = -MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float y_max = MAP_WIDTH_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float z_min = -MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float z_max = MAP_HEIGHT_VOXEL_NUM * VOXEL_RESOLUTION / 2;
float res = 0.2;  // Smaller res will get better tracking result but is slow.

float future_status[VOXEL_NUM][PREDICTION_TIMES];
Eigen::Vector3d map_center_point;
pcl::KdTreeFLANN<pcl::PointXYZ> kdtree; // kdtree to check collision

const float CAL_DURATION = 0.050f; // 20Hz
const float SEND_DURATION = 0.050f; //20Hz (10HZ at least)


const int ANGLE_H_NUM = 17;
const int ANGLE_V_NUM = 7; 

const int LOOKING_PIECES_SIZE = 16; // Should be even, Resolution: 22.5 degrees one piece. MID=7. Yaw=0. Larger: 7 - 15 :  0 to Pi;  Smaller: 7-0 : 0 to (nearly)-Pi;
float CAMERA_H_FOV = 62.f;  //degrees, in parameter list
const float HEAD_BUFFER_HIT_INCREASE = 0.4f;
const float HEAD_BUFFER_MISS_DECREASE_STANDARD_V = 0.05f; // Miss add value when velocity is 1m/s
const float HEAD_BUFFER_MISS_DECREASE_MIN = 0.03f;

float MAX_V_XY = 1.f;
float MAX_V_Z_UP = 1.f;
float MAX_V_Z_DOWN = 0.5f;
float MAX_A = 1.5f;
float HEIGHT_LIMIT = 2.f;
float XY_LIMIT = 2.5f;

float DIRECTION_CHANGE_LEFT_SIZE = 1.5;

float PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;

bool DIRECTION_AUTO_CHANGE = false;

struct  Path_Planning_Parameters
{
    float d_ref = 1.6;  //NOTE: NEED TO CHANGE CHG
    float k1_xy = 5; //% Goal directed coefficient
    float k1_z = 3; //% Goal directed coefficient
    float k2_xy = 1; //% Rotation coefficient 3
    float k2_z = 1.5; //% Rotation coefficient 3.5
    float v_max_at_goal_ori = 0.3; //% m/s, just reference  5.0 originally
    float v_scale_min = 0.1;
    float collision_threshold_static = 0.6;
    float collision_threshold_dynamic = 1.0;
    int buffer_send_counter = 0;
    int max_plan_num = ANGLE_H_NUM * ANGLE_V_NUM;  // Previously it was 100, as 18*7 = 126 > 100
}pp;


struct  Head_Planning_Parameters
{
   float k_current_v = 0.6;
   float k_planned_direction = 0.2;
   float k_v_fluctuation = 0.3;
   float k_dynamic_objects = 0.5;
   float k_common_update = 0.1;
}hp;

struct  Position_Tracker_Parameters
{
    float kp_xy = 0.2;
    float kp_z = 0.1;
    float kp_yaw = 0.2;
    float p_2_delt_v_max_xy = 0.8;
    float p_2_delt_v_max_z = 0.4;
    float max_yaw_rate = 1.0;
}pt;

float MAP_DELAY_SECONDS = 0.05;
const int point_num_pub = 5; // For the visualization of current planned trajectory

/*** End of Parameters ***/

/** Basic global variables **/
ros::Publisher current_marker_pub;
ros::Publisher cloud_pub, future_status_pub, goal_pub;
ros::Publisher map_center_pub;

ros::Publisher head_cmd_pub; // add on 9 Sept.
ros::Publisher motor_pub;
ros::Publisher pva_pub;  //add on 20 Aug. 2020


ros::Publisher sim_trajectory_pub; // add on 7 Jan. 2020

float x_centre, y_centre, z_centre;

bool if_in_simulation = false;

bool offboard_ready = false;
bool objects_updated = false;
bool imu_initilized = false;
bool state_locked = false;
bool state_updating = false;
bool safe_trajectory_avaliable = true;
bool use_position_global_time = false;
bool if_plan_vertical_path = true;
bool if_sim_lee_position_controller = false;
bool if_flyback_enable = true;


std::queue<double> pose_timestamp_queue;
std::queue<Eigen::Quaternionf> att_queue;
std::queue<Eigen::Vector3d> pos_queue;

/****** Global variables for path planning ******/
ros::Time _data_input_time;
ros::Time _algorithm_time;
float global_time_now;  //update in position callback

bool initialized = false;

Eigen::VectorXd Fov_half(2); //Fov parameters
Eigen::VectorXd Angle_h(ANGLE_H_NUM);  // initiate later in the main function, rad
Eigen::VectorXd Angle_v(ANGLE_V_NUM); // initiate later in the main function, rad

Eigen::Vector3d p_goal;
Eigen::Vector3d p_goal_raw;
Eigen::Vector3d p0;
Eigen::Vector3d v0;
Eigen::Vector3d a0;
Eigen::Quaternionf quad(1.0, 0.0, 0.0, 0.0);
Eigen::Vector3d init_p;
float collected_time;
float yaw0;
float yaw0_rate;
//Eigen::VectorXd quad(4);

float yaw_init = 0.0;  //Keep zero in this edition, the sampled directions are from all sides
float theta_h_last = 0.0;
float theta_v_last = 0.0;

Eigen::Vector3d p_store;
Eigen::Vector3d *send_traj_buffer_p;
Eigen::Vector3d *send_traj_buffer_v;
Eigen::Vector3d *send_traj_buffer_a;
int send_buffer_size = 0;
int send_buffer_size_max;

/*** Global variables for rotating head ***/
Eigen::VectorXf _direction_update_buffer; // Range [0, 1]
float heading_resolution;
int valid_piece_num_one_side;
int mid_seq_num; 

float motor_yaw = 0.0;
float motor_yaw_rate = 0.0;
float head_max_yaw_delt = 0.5;
float motor_velocity_set = 50.0;

float init_head_yaw = 0.0;


/** Publishers for cost visualization **/
bool if_publish_panel_arrays = true;
ros::Publisher cost_head_update_degree_pub;
ros::Publisher cost_head_velocity_pub;
ros::Publisher cost_head_direction_pub;
ros::Publisher cost_head_objects_pub;
ros::Publisher cost_head_fluctuation_pub;
ros::Publisher cost_head_final_pub;

/** Declaration of functions**/

void sendMotorCommands(float yaw);

// void trackVelocityPose(float vx_sp, float vy_sp, float vz_sp, float yaw_rate_sp,
//                     float px_sp_last, float py_sp_last, float pz_sp_last, float yaw_sp_last);

void setPointSendCallback();

void randomGoalGenerate();

void setParameters();

/**********************FOR HSH TABLE***************************/
// PVA table for feasible total planning time T
typedef struct pva_table pva_table;
struct pva_table {
    int dim_num;
    int *dim_size;
    uint64_t *dim_interval;
    uint64_t table_size;
    float *table;
    float rez;
    float *pva_limit;

    void construct_pva_table(int dim1_size, int dim2_size, int dim3_size, int dim4_size, float resolution) {
        this->dim_num = 4;
        this->dim_size = (int*)malloc(sizeof(int)*this->dim_num);
        this->dim_interval = (uint64_t*)malloc(sizeof(uint64_t)*this->dim_num);

        this->dim_size[0] = dim1_size;
        this->dim_size[1] = dim2_size;
        this->dim_size[2] = dim3_size;
        this->dim_size[3] = dim4_size;

        this->dim_interval[3] = 1;
        this->dim_interval[2] = dim_interval[3] * dim4_size;
        this->dim_interval[1] = dim_interval[2] * dim3_size;
        this->dim_interval[0] = dim_interval[1] * dim2_size;

        this->table_size = this->dim_interval[0] * dim1_size;
        this->table = (float*)malloc(sizeof(float)*this->table_size);

        this->rez = resolution;

        this->pva_limit = (float*)malloc(sizeof(float)*3);
        this->pva_limit[0] = this->rez*float(dim1_size/2);
        this->pva_limit[1] = this->rez*float(dim2_size/2);
        this->pva_limit[2] = this->rez*float(dim4_size/2);
    }

    void compute_idx_from_pva(float dlt_p, float v0, float vf, float a0,
                              int &idx1, int &idx2, int &idx3, int &idx4) {
        idx1 = round(dlt_p/this->rez) + this->dim_size[0]/2;
        idx2 = round(v0/this->rez) + this->dim_size[1]/2;
        idx3 = round(vf/this->rez) + this->dim_size[2]/2;
        idx4 = round(a0/this->rez) + this->dim_size[3]/2;
    }

    float query_pva_table(float dlt_p, float v0, float vf, float a0) {
        if (fabs(dlt_p) > this->pva_limit[0]) static_assert("Invalid input!", "");
        if (fabs(v0) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(vf) > this->pva_limit[1]) static_assert("Invalid input!", "");
        if (fabs(a0) > this->pva_limit[2]) static_assert("Invalid input!", "");

        int idx1, idx2, idx3, idx4;
        this->compute_idx_from_pva(dlt_p, v0, vf, a0, idx1, idx2, idx3, idx4);

        uint64_t idx = idx1*this->dim_interval[0] + idx2*this->dim_interval[1] +
                       idx3*this->dim_interval[2] + idx4*this->dim_interval[3];

        // std::cout << "idx: " << idx << std::endl;

        return this->table[idx];
    }

    void pva_table2csv(const std::string &str) {
        std::ofstream outfile;
        outfile.open(str, std::ios::out);

        for (int i = 0; i < 4; ++i) outfile << std::to_string(this->dim_size[i]) << ',';
        outfile << std::to_string(this->rez) << std::endl;

        for (uint64_t i = 0; i < this->table_size-1; ++i) outfile << std::to_string(this->table[i]) << ',';
        outfile << std::to_string(this->table[table_size-1]);

        outfile.close();
    }

    void csv2pva_table(const std::string &str) {
        int tmp_dim_size[4];
        float tmp_rez;

        std::ifstream infile(str, std::ios::in);
        std::string tmp_str;

        for (int i = 0; i < 4; ++i) {
            getline(infile, tmp_str, ',');
            tmp_dim_size[i] = std::stoi(tmp_str);
        }

        getline(infile, tmp_str);
        tmp_rez = std::stod(tmp_str);

        this->construct_pva_table(tmp_dim_size[0], tmp_dim_size[1],
                                  tmp_dim_size[2], tmp_dim_size[3], tmp_rez);

        for (uint64_t i = 0; i < this->table_size; ++i) {
            getline(infile, tmp_str, ',');
            this->table[i] = std::stod(tmp_str);
        }
    }

    void free_pva_table() {
        free(this->pva_limit);
        free(this->table);
        free(this->dim_interval);
        free(this->dim_size);
    }
};

pva_table *trajectory_time_table = (pva_table*)malloc(sizeof(pva_table));
bool table_initialized = false;

/***************************************************/


int inRange(float &low, float &high, float &x)
{
    if(x > low && x < high){
        return 1;
    }else{
        return 0;
    }
}

void colorAssign(int &r, int &g, int &b, float v, float value_min=0.f, float value_max=1.f, int reverse_color=0)
{
    v = std::max(v, value_min);
    v = std::min(v, value_max);

    float v_range = value_max - value_min;
    int value = floor((v - value_min) / v_range * 240); // Mapping 0~1.0 to 0~240
    value = std::min(value, 240);

    if(reverse_color){
        value = 240 - value;
    }

    int section = value / 60;
    float float_key = (value % 60) / (float)60 * 255;
    int key = floor(float_key);
    int nkey = 255 - key;

    switch(section) {
        case 0: // G increase
            r = 255;
            g = key;
            b = 0;
            break;
        case 1: // R decrease
            r = nkey;
            g = 255;
            b = 0;
            break;
        case 2: // B increase
            r = 0;
            g = 255;
            b = key;
            break;
        case 3: // G decrease
            r = 0;
            g = nkey;
            b = 255;
            break;
        case 4:
            r = 0;
            g = 0;
            b = 255;
            break;
        default: // White
            r = 255;
            g = 255;
            b = 255;
    }

}


float getHeadingYawFromSeq(int seq)
{
    if(seq >= LOOKING_PIECES_SIZE || seq < 0){
        ROS_ERROR("Seq for yaw buffer of the head out of range.");
        return 0.f;
    }
    else{
        return (seq - mid_seq_num) * heading_resolution;
    }
}

void correctAngleToPiRange(float &angle){
    while(angle > M_PI){
        angle = angle - 2*M_PI;
    }
    while(angle < -M_PI){
        angle = 2*M_PI + angle;
    }
}

int getHeadingSeq(float direction)
{
    if(direction > PI){
        direction -= PIx2;
    }
    else if(direction < -PI){
        direction += PIx2;
    } 

    if(direction > 0){  // To sovle the truncation problem
        direction += heading_resolution / 2.f;
    }
    else{
        direction -= heading_resolution / 2.f;  
    }
    /// Caution!!!!! the rotating range of the camera can only be within (-PI-heading_resolution, PI+heading_resolution), chg
    int heading_seq =  (int)(direction / heading_resolution) + mid_seq_num;
    if(heading_seq == LOOKING_PIECES_SIZE) heading_seq = 0;
    else if(heading_seq < 0) heading_seq = LOOKING_PIECES_SIZE - 1;

    return heading_seq;
}

void correctPieceSeq(int &seq)
{
    if(seq >= LOOKING_PIECES_SIZE){   // to form a ring
        seq = seq - LOOKING_PIECES_SIZE;
    }
    else if(seq < 0){
        seq = seq + LOOKING_PIECES_SIZE;
    }
}

void addHitOnePiece(int seq)
{
    //std::cout<<"hit seq = "<<seq<<std::endl;
    _direction_update_buffer(seq)+HEAD_BUFFER_HIT_INCREASE<=1.f ? _direction_update_buffer(seq)+=(float)HEAD_BUFFER_HIT_INCREASE : _direction_update_buffer(seq)=1.f;
}

void addMissOnePiece(int seq)
{
    float delt_miss = HEAD_BUFFER_MISS_DECREASE_MIN;
    float delt_miss_by_velocity = HEAD_BUFFER_MISS_DECREASE_STANDARD_V * std::max(fabs(v0(0)), fabs(v0(1)));
    if(delt_miss_by_velocity > delt_miss){
        delt_miss = delt_miss_by_velocity;
    }

    //std::cout<<"miss seq = "<<seq<<std::endl;
    _direction_update_buffer(seq)-delt_miss>=0.f ? _direction_update_buffer(seq)-=delt_miss : _direction_update_buffer(seq)=0.f;
}

void updateHeadBuffer(const int &heading_direction_seq)
{
    Eigen::VectorXi update_flag_buf = Eigen::VectorXi::Zero(LOOKING_PIECES_SIZE);

    //Add hit for the direction in FOV
    addHitOnePiece(heading_direction_seq);
    update_flag_buf(heading_direction_seq) = 1; 

    for(int i=1; i<=valid_piece_num_one_side; i++){
        int seq_this_1 = heading_direction_seq + i;
        int seq_this_2 = heading_direction_seq - i;
        
        correctPieceSeq(seq_this_1);
        correctPieceSeq(seq_this_2);

        addHitOnePiece(seq_this_1);
        addHitOnePiece(seq_this_2);

        update_flag_buf(seq_this_1) = 1;
        update_flag_buf(seq_this_2) = 1;
    }
    //Add miss for the rest
    for(int j=0; j<LOOKING_PIECES_SIZE; j++){
        if(update_flag_buf(j) != 1){
            addMissOnePiece(j);
        } 
    }
}


bool motor_initialized = false;
static bool cloud_init_time = true;

pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_filtered(new pcl::PointCloud<pcl::PointXYZ>());
// this callback use input cloud to update ring buffer, and update odometry of UAV
// void cloudCallback(const control_msgs::JointControllerStateConstPtr &motor_msg, const sensor_msgs::PointCloud2ConstPtr& cloud)
void cloudCallback(const sensor_msgs::PointCloud2ConstPtr& cloud)
{
    /*** For motor ***/
    float head_yaw_this = 0.f; /// For test
    if(cloud_init_time)
    {
        init_head_yaw = 0.0;
        cloud_init_time = false;
        motor_initialized = true;
        ROS_INFO("Head Init Yaw in motor coordinate=%f", init_head_yaw);
    }
    else
    {
        if(!if_in_simulation){  //real world
            motor_yaw =  head_yaw_this + init_head_yaw; // + PI_2?? //start with zero, original z for motor is down. now turn to ENU coordinate. Head forward is PI/2 ???????????
        }else{
            motor_yaw =  head_yaw_this - init_head_yaw;
        }
        motor_yaw_rate = 0.0;

        while(motor_yaw > PI){
            motor_yaw -= PIx2;
        }
        while(motor_yaw < -PI){
            motor_yaw += PIx2;
        }

        geometry_msgs::Point32 real_motor_pv;
        real_motor_pv.x = motor_yaw;
        real_motor_pv.y = motor_yaw_rate;
        real_motor_pv.z = 0;

        motor_pub.publish(real_motor_pv);
    }

    /*** For cloud ***/
    Eigen::Quaternionf quad_sychronized = quad;  //initialze in case no point in the queue satisfy
    Eigen::Vector3d current_position_sychronized = p0;

    static Eigen::Quaternionf quad_last_popped(-10.f, -10.f, -10.f, -10.f);
    static Eigen::Vector3d position_last_popped(-10000.f, -10000.f, -10000.f);
    static double last_popped_time = 0.0;

    while(!pose_timestamp_queue.empty()){   //Synchronize pose by queue
        double time_stamp_pose = pose_timestamp_queue.front();
        if(time_stamp_pose >= cloud->header.stamp.toSec()){
            quad_sychronized = att_queue.front();
            current_position_sychronized = pos_queue.front();

            // linear interpolation
            if(quad_last_popped.x() >= -1.f){
                double time_interval_from_last_time = time_stamp_pose - last_popped_time;
                double time_interval_cloud = cloud->header.stamp.toSec() - last_popped_time;
                double factor = time_interval_cloud / time_interval_from_last_time;
                quad_sychronized = quad_last_popped.slerp(factor, quad_sychronized);
                current_position_sychronized = position_last_popped * (1.0 - factor) + current_position_sychronized*factor;
            }

            ROS_INFO_THROTTLE(3.0, "cloud mismatch time = %lf", cloud->header.stamp.toSec() - time_stamp_pose);

            break;
        }

        quad_last_popped = att_queue.front();
        position_last_popped = pos_queue.front();
        last_popped_time = time_stamp_pose;

        pose_timestamp_queue.pop();
        att_queue.pop();
        pos_queue.pop();
    }


    // Add the rotation of head
    Eigen::Quaternionf axis_motor;
    axis_motor.w() = cos(motor_yaw/2.0);
    axis_motor.x() = 0;
    axis_motor.y() = 0;
    axis_motor.z() = sin(motor_yaw/2.0);
    Eigen::Quaternionf quad_rotate = quad_sychronized * axis_motor;


    /// Point cloud process
    static double last_time = ros::Time::now().toSec();
    double this_time = ros::Time::now().toSec();

    // convert cloud to pcl form
    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_in(new pcl::PointCloud<pcl::PointXYZ>());
    pcl::fromROSMsg(*cloud, *cloud_in);

    // down-sample for all
    pcl::VoxelGrid<pcl::PointXYZ> sor;
    sor.setInputCloud(cloud_in);
    sor.setLeafSize(res, res, res);
    sor.filter(*cloud_filtered);

    int useful_point_num = 0;
    for(int i=0; i<cloud_filtered->width; i++){
        float x = cloud_filtered->points.at(i).z;
        float y = -cloud_filtered->points.at(i).x;
        float z = -cloud_filtered->points.at(i).y;

        if(inRange(x_min, x_max, x) && inRange(y_min, y_max, y) && inRange(z_min, z_max, z))
        {
            point_clouds[useful_point_num*3] = x;
            point_clouds[useful_point_num*3+1] = y;
            point_clouds[useful_point_num*3+2] = z;
            ++ useful_point_num;

            if(useful_point_num >= MAX_POINT_NUM){
                break;
            }
        }
    }


    /// Update
    clock_t start1, finish1;
    start1 = clock();

    my_map.update(useful_point_num, 3, point_clouds,
                  current_position_sychronized.x(), current_position_sychronized.y(), current_position_sychronized.z(), this_time,
                  quad_rotate.w(), quad_rotate.x(), quad_rotate.y(), quad_rotate.z());

    map_center_point = current_position_sychronized;

    finish1 = clock();
    float duration1 = (float)(finish1 - start1) / CLOCKS_PER_SEC;
    ROS_INFO_THROTTLE( 1, "****** Map update time %f seconds\n", duration1);

    /// Get occupancy status
//    clock_t start2, finish2;
//    start2 = clock();

    int occupied_num=0;
    pcl::PointCloud<pcl::PointXYZ> cloud_to_publish;
    sensor_msgs::PointCloud2 cloud_to_pub_transformed;

    my_map.getOccupancyMapWithFutureStatus(occupied_num, cloud_to_publish, &future_status[0][0], 0.6);
//    my_map.getOccupancyMap(occupied_num, cloud_to_publish, 0.6);

    // U tree
//    pcl::PointCloud<pcl::PointXYZ>::Ptr cloud_to_publish_ptr = cloud_to_publish.makeShared();
//    kdtree.setInputCloud(cloud_to_publish_ptr);

    /// Publish Point cloud and center position
    pcl::PointCloud<pcl::PointXYZ> cloud_to_publish_moved;
    for(auto &p : cloud_to_publish.points){
        pcl::PointXYZ p2;
        p2.x = p.x; // + map_center_point.x();
        p2.y = p.y; // + map_center_point.y();
        p2.z = p.z; // + map_center_point.z();
        cloud_to_publish_moved.push_back(p2);
    }

    pcl::toROSMsg(cloud_to_publish_moved, cloud_to_pub_transformed);
    cloud_to_pub_transformed.header.frame_id = "map";
    cloud_to_pub_transformed.header.stamp = ros::Time::now();
    cloud_pub.publish(cloud_to_pub_transformed);

    geometry_msgs::PoseStamped map_pose;
    map_pose.header.stamp = cloud_to_pub_transformed.header.stamp;
    map_pose.pose.position.x = current_position_sychronized.x();
    map_pose.pose.position.y = current_position_sychronized.y();
    map_pose.pose.position.z = current_position_sychronized.z();
    map_pose.pose.orientation.x = quad_rotate.x();
    map_pose.pose.orientation.y = quad_rotate.y();
    map_pose.pose.orientation.z = quad_rotate.z();
    map_pose.pose.orientation.w = quad_rotate.w();
    map_center_pub.publish(map_pose);

    /// Publish future status of one layer
    pcl::PointCloud<pcl::PointXYZRGB> future_status_cloud;
    static const int z_index_to_show = MAP_HEIGHT_VOXEL_NUM / 2 - 1; ///Layer
    for(int j=0; j<MAP_WIDTH_VOXEL_NUM; ++j){
        for(int i=0; i<MAP_LENGTH_VOXEL_NUM; ++i){
            int index_this = z_index_to_show*MAP_WIDTH_VOXEL_NUM*MAP_LENGTH_VOXEL_NUM + j*MAP_WIDTH_VOXEL_NUM + i;

            for(int n=0; n<PREDICTION_TIMES; ++n){
                pcl::PointXYZRGB p_this;
                float x_offset = (float)n * 12.f; //Used to show prediction at different times in one map

                my_map.getVoxelPositionFromIndexPublic(index_this, p_this.x, p_this.y, p_this.z);
                p_this.x += x_offset;

                float weight_this = future_status[index_this][n];
                int r, g, b;
                colorAssign(r, g, b, weight_this, 0.f, 0.1f, 1);
                p_this.r = r;
                p_this.g = g;
                p_this.b = b;
                future_status_cloud.push_back(p_this);
            }
        }
    }

    sensor_msgs::PointCloud2 cloud_future_transformed;
    pcl::toROSMsg(future_status_cloud, cloud_future_transformed);
    cloud_future_transformed.header.frame_id = "map";
    cloud_future_transformed.header.stamp = ros::Time::now();
    future_status_pub.publish(cloud_future_transformed);

//    finish2 = clock();
//    float duration2 = (float)(finish2 - start2) / CLOCKS_PER_SEC;
//    printf( "****** Map publish time %f seconds\n \n", duration2);

    last_time = this_time;




    /* Update buffer for rolling head */
    double head_update_start_time = ros::Time::now().toSec();
    int heading_direction_seq = getHeadingSeq(motor_yaw);  //current heading direction
    updateHeadBuffer(heading_direction_seq);
    double head_update_time = ros::Time::now().toSec() - head_update_start_time;
    ROS_INFO_THROTTLE(3.0, "SUD update time = %lf", head_update_time);
}


float trajectoryNearestDistanceToObstacle(Eigen::Vector3d *traj, int num, float radius){
    float nearest_square_distance = 10000.f;
    static int check_neighbors_num = ceil(radius / VOXEL_RESOLUTION);

    for(int n=0; n<num; ++n) {
        Eigen::Vector3d p = traj[n] - map_center_point;

        if(traj[n].z() > HEIGHT_LIMIT || traj[n].z() < 0.4){ /// Height limit, chg
            nearest_square_distance = 0.f;
            break;
        }

        for(int i = -check_neighbors_num; i <= check_neighbors_num; ++i)        {
            for(int j = -check_neighbors_num; j <= check_neighbors_num; ++j){
                for(int k = -check_neighbors_num; k <= check_neighbors_num; ++k) {
                    Eigen::Vector3d p_check;
                    double delt_p_x = (double)i*VOXEL_RESOLUTION;
                    double delt_p_y = (double)j*VOXEL_RESOLUTION;
                    double delt_p_z = (double)k*VOXEL_RESOLUTION;
                    p_check.x() = p.x() + delt_p_x;
                    p_check.y() = p.y() + delt_p_y;
                    p_check.z() = p.z() + delt_p_z;

                    int index;
                    if(my_map.getPointVoxelsIndexPublic(p_check.x(), p_check.y(), p_check.z(), index))
                    {
                        if(future_status[index][0] > 0.5){
                            float dist = sqrt(delt_p_x*delt_p_x + delt_p_y*delt_p_y + delt_p_z*delt_p_z);
                            if(dist < nearest_square_distance){
                                nearest_square_distance = dist;
                            }
                        }
                    }

                }
            }
        }



    }

    return nearest_square_distance;
}


float getRiskOfTrajectory(Eigen::Vector3d *traj, int num){

    static const float check_time_1 = (prediction_future_time[1] + prediction_future_time[2]) / 2.f;
    static const float check_time_2 = (prediction_future_time[2] + prediction_future_time[3]) / 2.f;
    static int check_neighbors_num = ceil(pp.collision_threshold_static / VOXEL_RESOLUTION);

    float risk = 0.f;

    for(int n=0; n<num; ++n){
        Eigen::Vector3d p = traj[n] - map_center_point;
        float t = SEND_DURATION * (float)n;
        int risk_map_seq = 1;
        if(t > check_time_1){
            if(t > check_time_2){
                risk_map_seq = 3;
            }else{
                risk_map_seq = 2;
            }
        }

        for(int i = -check_neighbors_num; i <= check_neighbors_num; ++i)        {
            for(int j = -check_neighbors_num; j <= check_neighbors_num; ++j){
                for(int k = -check_neighbors_num; k <= check_neighbors_num; ++k) {
                    Eigen::Vector3d p_check;
                    p_check.x() = p.x() + (double)i*VOXEL_RESOLUTION;
                    p_check.y() = p.y() + (double)j*VOXEL_RESOLUTION;
                    p_check.z() = p.z() + (double)k*VOXEL_RESOLUTION;

                    int index;
                    if(my_map.getPointVoxelsIndexPublic(p_check.x(), p_check.y(), p_check.z(), index))
                    {
                        risk += future_status[index][risk_map_seq];
                    }

                }
            }
        }
    }

    return risk;
}


/** This function is to generate state to state trajectory **/
bool motion_primitives_with_table(Eigen::Vector3d p0, Eigen::Vector3d v0_ori, Eigen::Vector3d a0_ori, float yaw0, float theta_h,
                                  float theta_v, Eigen::Vector3d goal, float d, float v_max, float delt_t,
                                  Eigen::MatrixXd &p, Eigen::MatrixXd &v, Eigen::MatrixXd &a, Eigen::VectorXd &t)
{
    float delt_x = d*cos(theta_v)*cos(theta_h+yaw0);
    float delt_y = d*cos(theta_v)*sin(theta_h+yaw0);
    float delt_z = d*sin(theta_v);

    Eigen::Vector3d pf;
    pf << p0(0)+delt_x, p0(1)+delt_y, p0(2)+delt_z;

//    ROS_INFO_THROTTLE(0.2, "pf=(%f, %f, %f)", pf(0), pf(1), pf(2));
//    ROS_INFO_THROTTLE(0.2, "p0=(%f, %f, %f)", p0(0), p0(1), p0(2));

    Eigen::Vector3d l = goal - pf;
    Eigen::Vector3d vf = (v_max / l.norm()) * l;
    vf(2) = 0; // % Note: 0 maybe better, for the p curve wont go down to meet the vf

    Eigen::Vector3d af = Eigen::Vector3d::Zero();
    
    float j_limit = 4;
    float a_limit = MAX_A;
    float v_limit = MAX_V_XY;

    Eigen::Vector3d v0 = v0_ori;
    Eigen::Vector3d a0 = a0_ori;
    for(int i=0; i<3; i++){
        if(fabs(v0(i)) > MAX_V_XY) {
            v0(i) = MAX_V_XY * v0(i) / fabs(v0(i));
            ROS_WARN_THROTTLE(1, "v_limit in local planning is large than the values in the loaded table!  v0 max=%f", v0.cwiseAbs().maxCoeff());
        }
        if(fabs(a0(i)) > MAX_A) {
            a0(i) = MAX_A * a0(i) / fabs(a0(i));
            ROS_WARN_THROTTLE(1, "a_limit in local planning is large than the values in the loaded table!  a0 max=%f", a0.cwiseAbs().maxCoeff());
        }
    }

    float T1, T2, T3, T;
    T1 = trajectory_time_table->query_pva_table(delt_x, v0(0), vf(0), a0(0));
    T2 = trajectory_time_table->query_pva_table(delt_y, v0(0), vf(0), a0(0));
    T3 = trajectory_time_table->query_pva_table(delt_z, v0(0), vf(0), a0(0));

    if (T1 == -1 || T2 == -1 || T3 == -1) {
        return false;
    }

    T = T1 > T2 ? T1 : T2;
    T = T > T3 ? T : T3;
    T *= 1.1;
    T = T < 0.5 ? 0.5 : T;
    
    int times = T / delt_t;

    p = Eigen::MatrixXd::Zero(times, 3);
    v = Eigen::MatrixXd::Zero(times, 3);
    a = Eigen::MatrixXd::Zero(times, 3);
    t = Eigen::VectorXd::Zero(times);

    // % calculate optimal jerk controls by Mark W. Miller
    for(int ii=0; ii<3; ii++)
    {
        float delt_a = af(ii) - a0(ii);
        float delt_v = vf(ii) - v0(ii) - a0(ii)*T;
        float delt_p = pf(ii) - p0(ii) - v0(ii)*T - 0.5*a0(ii)*T*T;

        // % if vf is not free
        float alpha = delt_a*60/pow(T,3) - delt_v*360/pow(T,4) + delt_p*720/pow(T,5);
        float beta = -delt_a*24/pow(T,2) + delt_v*168/pow(T,3) - delt_p*360/pow(T,4);
        float gamma = delt_a*3/T - delt_v*24/pow(T,2) + delt_p*60/pow(T,3);

        for(int jj=0; jj<times; jj++)
        {
            float tt = (jj + 1)*delt_t;
            t(jj) = tt;
            p(jj,ii) = alpha/120*pow(tt,5) + beta/24*pow(tt,4) + gamma/6*pow(tt,3) + a0(ii)/2*pow(tt,2) + v0(ii)*tt + p0(ii);
            v(jj,ii) = alpha/24*pow(tt,4) + beta/6*pow(tt,3) + gamma/2*pow(tt,2) + a0(ii)*tt + v0(ii);
            a(jj,ii) = alpha/6*pow(tt,3) + beta/2*pow(tt,2) + gamma*tt + a0(ii);

            // // Only for checking acceleration
            // if (fabs(a(jj, ii)) > planning_max_accel(ii)) planning_max_accel(ii) = fabs(a(jj, ii));
            // if (fabs(v(jj, ii)) > planning_max_vel(ii)) planning_max_vel(ii) = fabs(v(jj, ii));
        }
    }

    return true;
}


/* Publish markers to show the path in rviz */
void marker_publish(Eigen::MatrixXd &Points)
{
    visualization_msgs::Marker points, goal_strip;
    goal_strip.header.frame_id = points.header.frame_id = "map";
    goal_strip.header.stamp = points.header.stamp = ros::Time::now();
    goal_strip.action = points.action = visualization_msgs::Marker::ADD;
    goal_strip.ns=points.ns = "lines_and_points";
    points.id = 1;
    goal_strip.id = 0;
    goal_strip.type = visualization_msgs::Marker::LINE_STRIP;
    points.type = visualization_msgs::Marker::POINTS;


    // Line width
    points.scale.x = 0.1;
    points.scale.y = 0.1;

    goal_strip.scale.x = 0.05;
    goal_strip.scale.y = 0.05;


    // points are green
    points.color.r = 0.85;
    points.color.g = 0.85;
    points.color.b = 0.85;
    points.color.a = 1.0;
    points.lifetime = ros::Duration(0);

    goal_strip.color.g = 0.8;
    goal_strip.color.b = 0.8;
    goal_strip.color.r = 1.0;
    goal_strip.color.a = 1.0;
    goal_strip.lifetime = ros::Duration(0);

    int point_num = Points.rows();

    /** Add current position **/
    geometry_msgs::Point p;
    p.x = p0(0) - map_center_point.x();
    p.y = p0(1) - map_center_point.y();
    p.z = p0(2) - map_center_point.z();
    points.points.push_back(p);
    goal_strip.points.push_back(p);

    /** Add planned path **/
    for(int i=0; i<point_num; i++)
    {
        geometry_msgs::Point p;
        p.x = Points(i, 0) - map_center_point.x();
        p.y = Points(i, 1) - map_center_point.y();
        p.z = Points(i, 2) - map_center_point.z();

        points.points.push_back(p);
    }

    current_marker_pub.publish(points);

//    geometry_msgs::Point p2;
//    p2.x = p0(0);
//    p2.y = p0(1);
//    p2.z = p0(2);
//    goal_strip.points.push_back(p2);
//    p2.x = p_goal(0);
//    p2.y = p_goal(1);
//    p2.z = p_goal(2);
//    goal_strip.points.push_back(p2);
//    current_marker_pub.publish(goal_strip);
}


/** Delt yaw calculation. Avoid +=Pi problem **/
float deltYawAbs(float &yaw1, float &yaw2)
{
    //std::cout << "(yaw1, yaw1)=" << yaw1 << "," << yaw2 <<std::endl;
    float delt_yaw = fabs(yaw1 - yaw2);
    if(delt_yaw > PI){
        delt_yaw = PIx2 - delt_yaw;
    }
    return delt_yaw;
}

/** This is the function to generate the collision-free path, which is trigered by a timer defined in main function. **/
void trajectoryCallback(const ros::TimerEvent& e) {
    /** Generate trajectory for uav first***/
    float theta_h_chosen;
    int collision_check_pass_flag = 1;

    if(!state_updating && !cloud_init_time) /// Need to rethink the necessity of this lock!!!! CHG
    {
        state_locked = true;

        double trajectory_planning_start_time = ros::Time::now().toSec();

        /** Moition primitives **/
        Eigen::Vector3d delt_p = p_goal - p0;

        float phi_h = atan2(delt_p(1), delt_p(0)); //% horizental offset angle
        float phi_v = atan2(delt_p(2), sqrt(delt_p(0) * delt_p(0) + delt_p(1) * delt_p(1))); //% vertical offset angle

        // %calculate cost for sampled points
        Eigen::MatrixXd cost = Eigen::MatrixXd::Zero(ANGLE_H_NUM * ANGLE_V_NUM, 4);
        float theta_h = 0;
        float theta_v = 0;

        for(int i=0; i<ANGLE_H_NUM; i++)
        {
            for(int j=0; j<ANGLE_V_NUM; j++)
            {
                theta_h = Angle_h(i) + phi_h;  //start from goal position, chg, 2019.8.31
                theta_v = Angle_v(j) + phi_v;  //start from goal position, chg, 2019.8.31
                int m = i*ANGLE_V_NUM + j; //sequence number
                // Vectorial angle can never be larger than PI
                float goal_diff_h = Angle_h(i);
                float goal_diff_v = Angle_v(j);
                correctAngleToPiRange(theta_h);
                correctAngleToPiRange(theta_v);

                cost(m, 0) = pp.k1_xy*goal_diff_h*goal_diff_h + pp.k1_z*goal_diff_v*goal_diff_v +
                        pp.k2_xy*(theta_h-theta_h_last)*(theta_h-theta_h_last) + pp.k2_z*(theta_v-theta_v_last)*(theta_v-theta_v_last);
                cost(m, 1) = theta_h;
                cost(m, 2) = theta_v;
                cost(m, 3) = pp.d_ref;
            }
        }

        //% Rank by cost, small to large
        for(int m=0; m<ANGLE_H_NUM*ANGLE_V_NUM-1; m++)
        {
            for(int n=0; n<ANGLE_H_NUM*ANGLE_V_NUM-m-1; n++)
            {
                if(cost(n,0) > cost(n+1,0))
                {
                    Eigen::Vector4d temp = cost.row(n+1);
                    cost.row(n+1) = cost.row(n);
                    cost.row(n) = temp;
                }
            }
        }


        //% max velocity is decreased concerning current velocity direction and goal
        //% direction
        float v_scale = std::max((float)(delt_p.dot(v0)/v0.norm()/delt_p.norm()), pp.v_scale_min);
        // If v0.norm() = 0, v_scale = nan
        v_scale = (v0.norm() == 0) ? pp.v_scale_min : v_scale;
        float v_max = pp.v_max_at_goal_ori * v_scale;

        theta_h_chosen = theta_h_last; //if the uav is in safe mode, this would be the last value so the head wont rotate

        static bool first_plan = true;
        if(first_plan){
            send_traj_buffer_p[pp.buffer_send_counter] = p0;
            send_traj_buffer_v[pp.buffer_send_counter] = v0;
            send_traj_buffer_a[pp.buffer_send_counter] = a0;
            first_plan = false;
        }

        int selected_seq = 0;
        bool found_feasible_path_directly = false;

        std::vector<int> valid_seq_vector;
        std::vector<float> valid_seq_future_risk_vector;

        for(int seq=0; seq<pp.max_plan_num; seq ++)
        {
            Eigen::MatrixXd p;
            Eigen::MatrixXd v;
            Eigen::MatrixXd a;
            Eigen::VectorXd t;

            // Use next desire point to plan if tracking is good, otherwise use a mixed state
            if((send_traj_buffer_p[pp.buffer_send_counter] - p0).squaredNorm() < PLAN_INIT_STATE_CHANGE_THRESHOLD){
                if(!motion_primitives_with_table(send_traj_buffer_p[pp.buffer_send_counter], send_traj_buffer_v[pp.buffer_send_counter], send_traj_buffer_a[pp.buffer_send_counter], 0, cost(seq,1),
                                  cost(seq,2), p_goal, cost(seq,3), v_max, SEND_DURATION, p, v, a, t)){
                    ROS_INFO_THROTTLE(3.0, "### Invalid planning time.");
                    continue;
                }
                ROS_INFO_THROTTLE(3.0, "### Continue to plan!");

            }else{
                if(!motion_primitives_with_table(send_traj_buffer_p[pp.buffer_send_counter]*0.1 + p0*0.9, send_traj_buffer_v[pp.buffer_send_counter]*0.1+v0*0.9, a0, 0, cost(seq,1),
                    cost(seq,2), p_goal, cost(seq,3), v_max, SEND_DURATION, p, v, a, t)){
                    ROS_INFO_THROTTLE(3.0, "### Invalid planning time.");
                    continue;
                }
                ROS_INFO_THROTTLE(3.0, "### Track point too far, replan!");
            }

            // Get points number on the path
            const int Num = p.rows();
            Eigen::Vector3d *sim_traj = new Eigen::Vector3d[Num];

            for (int i = 0; i < Num; i+=1) {
                sim_traj[i](0) = (float)p.row(i)(0);
                sim_traj[i](1) = (float)p.row(i)(1);
                sim_traj[i](2) = (float)p.row(i)(2);
            }

            /// TODO: MODIFY collision checking
            if(DIRECTION_AUTO_CHANGE){
//                collision_check_pass_flag = rrb.collision_checking_with_fence(sim_traj, Num, pp.collision_threshold_static, init_p, HEIGHT_LIMIT, XY_LIMIT);
                ROS_ERROR_THROTTLE(1, "We don't support DIRECTION_AUTO_CHANGE for now!");
            }else{
                float min_distance_to_obstalce = trajectoryNearestDistanceToObstacle(sim_traj, Num, 0.6);
                ROS_INFO_THROTTLE(0.3,"min_square_distance_to_obstalce=%f", min_distance_to_obstalce);

                if(min_distance_to_obstalce > pp.collision_threshold_static){
                    float future_risk = getRiskOfTrajectory(sim_traj, Num);
                    ROS_INFO_THROTTLE(0.3,"future_risk=%f", future_risk);
                    if(future_risk < 20.f){
                        selected_seq = seq;
                        found_feasible_path_directly = true;
                        break;
                    }

                    valid_seq_vector.push_back(seq);
                    valid_seq_future_risk_vector.push_back(future_risk);
                }

            }

            ROS_WARN_THROTTLE(1, "collision_check_pass_flag = %d", collision_check_pass_flag);
        }


        if(!valid_seq_vector.empty() || found_feasible_path_directly){
            collision_check_pass_flag = 1;

            /** Find the path with smallest risk ***/
            static float risk_coefficient = 1.0;
            static float multi_goal_cost_coefficient = 10.0;

            if(!found_feasible_path_directly){
                selected_seq = valid_seq_vector[0];
                float minimum_cost = risk_coefficient*valid_seq_future_risk_vector[0] + multi_goal_cost_coefficient*cost(selected_seq, 0);
                // ROS_INFO_THROTTLE(1.0, "6666 risk %f, cost %f", valid_seq_future_risk_vector[0], cost(selected_seq, 0));
                for(int ii=1; ii<valid_seq_vector.size(); ++ii){
                    int seq_this = valid_seq_vector[ii];
                    float cost_this = risk_coefficient*valid_seq_future_risk_vector[ii] + multi_goal_cost_coefficient*cost(seq_this, 0);

                    if(valid_seq_future_risk_vector[ii] < minimum_cost){
                        minimum_cost = valid_seq_future_risk_vector[ii];
                        selected_seq = valid_seq_vector[ii];
                    }
                }
            }

            /** Generate trajectory with the finally selected seq **/
            Eigen::MatrixXd p_selected;
            Eigen::MatrixXd v_selected;
            Eigen::MatrixXd a_selected;
            Eigen::VectorXd t_selected;

            // motion_primitives_with_table(send_traj_buffer_p[0], send_traj_buffer_v[0], send_traj_buffer_a[0], 0, cost(selected_seq,1),
            //                              cost(selected_seq,2), p_goal, cost(selected_seq,3), v_max, SEND_DURATION, p_selected, v_selected, a_selected, t_selected);

            if((send_traj_buffer_p[pp.buffer_send_counter] - p0).squaredNorm() < PLAN_INIT_STATE_CHANGE_THRESHOLD){
                motion_primitives_with_table(send_traj_buffer_p[pp.buffer_send_counter], send_traj_buffer_v[pp.buffer_send_counter], send_traj_buffer_a[pp.buffer_send_counter], 0, cost(selected_seq,1),
                                  cost(selected_seq,2), p_goal, cost(selected_seq,3), v_max, SEND_DURATION, p_selected, v_selected, a_selected, t_selected);

            }else{
                motion_primitives_with_table(send_traj_buffer_p[pp.buffer_send_counter]*0.1 + p0*0.9, send_traj_buffer_v[pp.buffer_send_counter]*0.1+v0*0.9, a0, 0, cost(selected_seq,1),
                    cost(selected_seq,2), p_goal, cost(selected_seq,3), v_max, SEND_DURATION, p_selected, v_selected, a_selected, t_selected);
            }


            int Num = p_selected.rows();
//ROS_INFO("traj_safe");
            safe_trajectory_avaliable = true;

            theta_h_chosen = cost(selected_seq,1);
            theta_h_last = cost(selected_seq,1); // Update last theta
            theta_v_last = cost(selected_seq,2);

            // Update the buffer to publish when it's not in emergency mode. Consider data lock for multi-thread calculation
            send_buffer_size = p_selected.rows();
            if(send_buffer_size > send_buffer_size_max) send_buffer_size = send_buffer_size_max;

            for(int point_seq=0; point_seq<send_buffer_size; point_seq++)
            {
                send_traj_buffer_p[point_seq](0) = (float)p_selected.row(point_seq)(0);
                send_traj_buffer_p[point_seq](1) = (float)p_selected.row(point_seq)(1);
                send_traj_buffer_p[point_seq](2) = (float)p_selected.row(point_seq)(2);

                send_traj_buffer_v[point_seq](0) = (float)v_selected.row(point_seq)(0);
                send_traj_buffer_v[point_seq](1) = (float)v_selected.row(point_seq)(1);
                send_traj_buffer_v[point_seq](2) = (float)v_selected.row(point_seq)(2);

                send_traj_buffer_a[point_seq](0) = (float)a_selected.row(point_seq)(0);
                send_traj_buffer_a[point_seq](1) = (float)a_selected.row(point_seq)(1);
                send_traj_buffer_a[point_seq](2) = (float)a_selected.row(point_seq)(2);
            }

            //Publish down sampled path points
            int interval_num = Num / point_num_pub;
            if (interval_num > 0)
            {
                Eigen::MatrixXd show_points = Eigen::MatrixXd::Zero(point_num_pub+1, 3);
                for(int pubi = 0; pubi < point_num_pub; pubi++)
                {
                    show_points.row(pubi) = p_selected.row(pubi*interval_num);
                }
                show_points.row(point_num_pub) = p_selected.row(Num-1);
                marker_publish(show_points);
            }
        }else {
            // Safety mode
            ROS_WARN_THROTTLE(2.0, "No valid trajectory found! Trapped in safety mode!");
            collision_check_pass_flag = 0;
            safe_trajectory_avaliable = false;

            // Publish points of stored point to show
            Eigen::MatrixXd show_points = Eigen::MatrixXd::Zero(1, 3);
            show_points(0, 0) = p0(0);
            show_points(0, 1) = p0(1);
            show_points(0, 2) = p0(2);
            marker_publish(show_points);
        }

        double algo_time = ros::Time::now().toSec() - _algorithm_time.toSec();
        ROS_INFO_THROTTLE(3.0, "path planning algorithm time is: %lf", algo_time);
        _algorithm_time = ros::Time::now();


        ROS_INFO_THROTTLE(0.3, " **** Trajectory planning takes %f s", ros::Time::now().toSec()-trajectory_planning_start_time);

        state_locked = false;
    }

    setPointSendCallback();  /// Send control setpoint



    /** Generate trajectory for rotating head **/
    static float last_head_yaw_plan = 0.0;
    static bool first_head_control_flag = true;

    double head_planning_start_time = ros::Time::now().toSec();

    if(first_head_control_flag){
        if(motor_initialized){
            first_head_control_flag = false;
            sendMotorCommands(0.0);
        }
    }
    else{
        /// Rank by the cost given from current velocity direction, the sampled last direction, update degree and the last planned head direction
        Eigen::VectorXd cost_vector = Eigen::VectorXd::Zero(LOOKING_PIECES_SIZE);

        /// Current velocity direction(v_direction), planned velocity direction(theta_h_chosen), yaw of the head must be in the same coordinate!!
        float yaw_to_send;

        /// Larger update degree indicates the influence of this item

        float k_current_v = 0;
        static float v_direction = 0;
        if(fabs(v0(0)) > 0.05 || fabs(v0(1) > 0.05)){  // if velocity is large, use this cost from velocity direction
            // Give a discount on hp.k_current_v (max) so that when the velocity of the drone is low, the influence of the velocity direction is small.
            k_current_v =  std::max(std::min(std::max(fabsf(v0(0)/MAX_V_XY), fabsf(v0(1)/MAX_V_XY)), 1.f), 0.f);
            v_direction = atan2(v0(1), v0(0));
        }
        float coefficient_current_v =  k_current_v * (1.0 - _direction_update_buffer(getHeadingSeq(v_direction)));

        float coefficient_planned_dir = 0.0;
        if(collision_check_pass_flag > 0) coefficient_planned_dir = 1.0 - _direction_update_buffer(getHeadingSeq(theta_h_chosen));


        /** Visualization variables **/
        std_msgs::Float64MultiArray cost_current_velocity_array;
        std_msgs::Float64MultiArray cost_planned_direction_array;
        std_msgs::Float64MultiArray cost_head_fluctuation_array;
        std_msgs::Float64MultiArray cost_dynamic_objects_array;
        std_msgs::Float64MultiArray cost_total_array;
        std_msgs::Float64MultiArray cost_update_array;
        /** End of visualization **/

        float min_head_plan_cost = 10000000.0;  //smaller cost is better
        for(int i=0; i<LOOKING_PIECES_SIZE; i++){
            float head_yaw_plan_temp = getHeadingYawFromSeq(i);

            float cost_current_velocity = hp.k_current_v * coefficient_current_v * (v_direction-head_yaw_plan_temp) * (v_direction-head_yaw_plan_temp);

            float cost_update_degree = hp.k_common_update * _direction_update_buffer(i);

            float cost_planned_direction = hp.k_planned_direction  * coefficient_planned_dir * (theta_h_chosen-head_yaw_plan_temp) * (theta_h_chosen-head_yaw_plan_temp);
            float cost_head_fluctuation = hp.k_v_fluctuation * (head_yaw_plan_temp - last_head_yaw_plan) * (head_yaw_plan_temp - last_head_yaw_plan);

            float cost_dynamic_objects = 0.0;
            float cost_dynamic_objects_min = - hp.k_dynamic_objects * 2;

            static int effect_range_one_side = 3;
            static float cost_one_piece = hp.k_dynamic_objects / effect_range_one_side;


            cost_dynamic_objects = std::max(cost_dynamic_objects, cost_dynamic_objects_min);

            float cost_total_temp = cost_current_velocity + cost_planned_direction + cost_head_fluctuation + cost_dynamic_objects + cost_update_degree;

	        /*** For visualization **/
            if(if_publish_panel_arrays){
                cost_update_array.data.push_back(cost_update_degree);
                cost_current_velocity_array.data.push_back(cost_current_velocity);
                cost_planned_direction_array.data.push_back(cost_planned_direction);
                cost_head_fluctuation_array.data.push_back(cost_head_fluctuation);
                cost_dynamic_objects_array.data.push_back(cost_dynamic_objects);
                cost_total_array.data.push_back(cost_total_temp);
            }
            /** End of visualization **/

            if(cost_total_temp < min_head_plan_cost)
            {
                min_head_plan_cost = cost_total_temp;
                yaw_to_send = head_yaw_plan_temp;
            }
        }

        double head_planning_update_time = ros::Time::now().toSec() - head_planning_start_time;
        ROS_INFO_THROTTLE(3.0, "head planning algorithm time is: %lf", head_planning_update_time);


        /*** For visualization **/
        if(if_publish_panel_arrays){
            cost_head_update_degree_pub.publish(cost_update_array);
            cost_head_velocity_pub.publish(cost_current_velocity_array);
            cost_head_direction_pub.publish(cost_planned_direction_array);
            cost_head_objects_pub.publish(cost_dynamic_objects_array);
            cost_head_fluctuation_pub.publish(cost_head_fluctuation_array);
            cost_head_final_pub.publish(cost_total_array);
        }

        /** End of visualization **/
        sendMotorCommands(yaw_to_send); //send to motor

        last_head_yaw_plan = yaw_to_send;
    }
}

void setPointSendCallback(){

    static Eigen::Vector3d p_store_for_em;
    
    static bool init_flag = true;
    if(init_flag){
        p_store_for_em << p0(0), p0(1), p0(2);
        init_flag = false;
    }

    static float def_ori = pp.d_ref;
    
    trajectory_msgs::JointTrajectoryPoint pva_setpoint;

    if(safe_trajectory_avaliable){
        pp.d_ref = def_ori;

        float z_p_set, z_v_set, z_a_set;
        if(if_plan_vertical_path){
            z_p_set = send_traj_buffer_p[pp.buffer_send_counter](2);
            z_v_set = send_traj_buffer_v[pp.buffer_send_counter](2);
            z_a_set = send_traj_buffer_a[pp.buffer_send_counter](2);
        } 
        else{
            z_p_set = p_goal(2);
            z_v_set = 0.f;
            z_a_set = 0.f;
        } 

        pva_setpoint.positions.push_back(send_traj_buffer_p[pp.buffer_send_counter](0)); //x
        pva_setpoint.positions.push_back(send_traj_buffer_p[pp.buffer_send_counter](1)); //y
        pva_setpoint.positions.push_back(z_p_set); //z
        pva_setpoint.positions.push_back(0);  //yaw

        pva_setpoint.velocities.push_back(send_traj_buffer_v[pp.buffer_send_counter](0));
        pva_setpoint.velocities.push_back(send_traj_buffer_v[pp.buffer_send_counter](1));
        pva_setpoint.velocities.push_back(z_v_set);

        pva_setpoint.accelerations.push_back(send_traj_buffer_a[pp.buffer_send_counter](0));
        pva_setpoint.accelerations.push_back(send_traj_buffer_a[pp.buffer_send_counter](1));
        pva_setpoint.accelerations.push_back(z_a_set);

        p_store_for_em << p0(0), p0(1), p0(2);

    }else{  //emergency stop
        pp.d_ref = 0.5;
        //trackVelocityPose(0.f, 0.f, 0.f, 0.f, p_store_for_em(0), p_store_for_em(1), p_store_for_em(2), yaw_init);

        pva_setpoint.positions.push_back(p_store_for_em(0)); //x
        pva_setpoint.positions.push_back(p_store_for_em(1)); //y
        pva_setpoint.positions.push_back(p_store_for_em(2)); //z
        pva_setpoint.positions.push_back(0);  //yaw

        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);
        pva_setpoint.velocities.push_back(0);

        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);
        pva_setpoint.accelerations.push_back(0);

    }

    pva_pub.publish(pva_setpoint);

}

void positionCallback(const geometry_msgs::PoseStamped& msg)
{
    if(use_position_global_time){
        global_time_now = msg.header.stamp.toSec();
    }

    if(!state_locked)
    {
        state_updating = true;

        /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
        // p0(0) = msg.pose.position.y;
        // p0(1) = -msg.pose.position.x;
        // p0(2) = msg.pose.position.z;

        // quad.x() = msg.pose.orientation.x;
        // quad.y() = msg.pose.orientation.y;
        // quad.z() = msg.pose.orientation.z;
        // quad.w() = msg.pose.orientation.w;

        // //Eigen::Quaternionf q1(0, 0, 0, 1);
        // Eigen::Quaternionf axis; //= quad * q1 * quad.inverse();
        // axis.w() = cos(-PI_2/2.0);
        // //axis.x() = axis.x() * sin(-PI_2/2.0);
        // //axis.y() = axis.y() * sin(-PI_2/2.0);
        // //axis.z() = axis.z() * sin(-PI_2/2.0);
        // axis.x() = 0.0;
        // axis.y() = 0.0;
        // axis.z() = sin(-PI_2/2.0);
        // quad = quad * axis;

        // NWU
        p0(0) = msg.pose.position.x;
        p0(1) = msg.pose.position.y;
        p0(2) = msg.pose.position.z;

        quad.x() = msg.pose.orientation.x;
        quad.y() = msg.pose.orientation.y;   
        quad.z() = msg.pose.orientation.z;
        quad.w() = msg.pose.orientation.w;

        // Queue for synchronization
        pose_timestamp_queue.push(msg.header.stamp.toSec());
        pos_queue.push(p0);
        att_queue.push(quad);

        if(pose_timestamp_queue.size()>200){
            pose_timestamp_queue.pop();
            pos_queue.pop();
            att_queue.pop();
        }

        
        /// Update yaw0 here, should be among [-PI, PI] 
        // yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));   /// TODO: CHECK if this is right!!!
        yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));


        state_updating = false;
    }

    if(!offboard_ready){
        init_p = p0;  //record a base point
        p_goal(0) = p_goal_raw(0) + init_p(0);
        p_goal(1) = p_goal_raw(1) + init_p(1);
        p_goal(2) = init_p(2);
        ROS_INFO_THROTTLE(1.0, "Waiting for offboard mode. p_goal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
    }else{
        ROS_INFO_THROTTLE(1.0, "IN offboard mode. pgoal = %f, %f, %f", p_goal(0), p_goal(1), p_goal(2));
    }

    ROS_INFO_THROTTLE(1.0, "Current pose. p0 = %f, %f, %f", p0(0), p0(1), p0(2));

    if(fabs(p0(1) - init_p(1))>DIRECTION_CHANGE_LEFT_SIZE && (p0(1)-init_p(1))*(p_goal(1)-init_p(1)) > 0 && DIRECTION_AUTO_CHANGE){
    //if(fabs(p0(1))>DIRECTION_CHANGE_LEFT_SIZE && p0(1)*p_goal(1) > 0 && DIRECTION_AUTO_CHANGE){
        randomGoalGenerate();
    }
}

void velocityCallback(const geometry_msgs::TwistStamped& msg)
{
    if(!state_locked)
    {
        state_updating = true;

        // /** Change from ENU to NWU, NEEDS CAREFUL CHECKING!!!!, chg**/
        // v0(0) = msg.twist.linear.y;
        // v0(1) = -msg.twist.linear.x;
        // v0(2) = msg.twist.linear.z;
        // yaw0_rate = msg.twist.angular.z;

        /**  NWU  **/
        v0(0) = msg.twist.linear.x;
        v0(1) = msg.twist.linear.y;
        v0(2) = msg.twist.linear.z;
        yaw0_rate = msg.twist.angular.z;

    	if(fabs(v0(0)) < 0.05) v0(0) = 0.0;  //add a dead zone for v0 used in motion primatives
    	if(fabs(v0(1)) < 0.05) v0(1) = 0.0;  //add a dead zone for v0 used in motion primatives
    	if(fabs(v0(2)) < 0.05) v0(2) = 0.0;  //add a dead zone for v0 used in motion primatives

        float max_current_v = 0.8;
        for(int i=0; i<3; i++){
            if(v0(i) < -max_current_v){
                v0(i) = -max_current_v;
            }else if(v0(i) > max_current_v){
                v0(i) = max_current_v;
            }
        }

    	/** Calculate virtual accelerates from velocity. Original accelerates given by px4 is too noisy **/
        static bool init_v_flag = true;
    	static float last_time, last_vx, last_vy, last_vz;
    	
    	if(init_v_flag){
    		init_v_flag = false;
    	}
    	else{
    		double delt_t = ros::Time::now().toSec() - last_time;
    		a0(0) = (v0(0) - last_vx) / delt_t;
    		a0(1) = (v0(1) - last_vy) / delt_t;
    		a0(2) = (v0(2) - last_vz) / delt_t;

    		if(fabs(a0(0)) < 0.2) a0(0) = 0.0;  //dead zone for acc x
     		if(fabs(a0(1)) < 0.2) a0(1) = 0.0; //dead zone for acc y
    		if(fabs(a0(2)) < 0.2) a0(2) = 0.0; //dead zone for acc z

            float max_current_a = 1.0;
            for(int i=0; i<3; i++){
                if(a0(i) < -max_current_a){
                    a0(i) = -max_current_a;
                }else if(a0(i) > max_current_a){
                    a0(i) = max_current_a;
                }
            }

    		//ROS_INFO("acc=(%f, %f, %f)", a0(0), a0(1), a0(2));
    	}

    	last_time = ros::Time::now().toSec();
    	last_vx = v0(0);
    	last_vy = v0(1);
    	last_vz = v0(2);

        state_updating = false;
    }
}

default_random_engine random_generator;
uniform_real_distribution<float> x_distribution(-1.3, 1.3);
uniform_real_distribution<float> z_distribution(0.7, 1.5);
void randomGoalGenerate()
{
    ROS_WARN("Last Goal = (%f, %f, %f)", p_goal(0), p_goal(1), p_goal(2));

    p_goal(0) = x_distribution(random_generator) + init_p(0);
    // p_goal(0) = init_p(0);
    // p_goal(2) = z_distribution(random_generator);  ///CHG!! 20200820  banned random generation of p_goal(2)

    if(p0(1) > 0){
        p_goal(1) = -4 + init_p(1);
    }else{
        p_goal(1) = 4 + init_p(1);
    }

    //publish center
    geometry_msgs::PointStamped goal_p;
    goal_p.header.stamp = ros::Time::now();
    goal_p.header.frame_id = "map";
    goal_p.point.x = p_goal(0);
    goal_p.point.y = p_goal(1);
    goal_p.point.z = p_goal(2);
    goal_pub.publish(goal_p);

    ROS_WARN("New Goal = (%f, %f, %f)", p_goal(0), p_goal(1), p_goal(2));
}

void simPositionVelocityCallback(const nav_msgs::Odometry &msg)
{
    if(use_position_global_time){
        global_time_now = msg.header.stamp.toSec();
    }
    collected_time = msg.header.stamp.toSec();

    if(!state_locked)
    {
        state_updating = true;

        /** NWU **/
        p0(0) = msg.pose.pose.position.x;
        p0(1) = msg.pose.pose.position.y;
        p0(2) = msg.pose.pose.position.z;

        quad.x() = msg.pose.pose.orientation.x;
        quad.y() = msg.pose.pose.orientation.y;
        quad.z() = msg.pose.pose.orientation.z;
        quad.w() = msg.pose.pose.orientation.w;
//        quad.y() = msg.pose.pose.orientation.x;
//        quad.x() = -msg.pose.pose.orientation.y;
//        quad.z() = msg.pose.pose.orientation.z;
//        quad.w() = msg.pose.pose.orientation.w;

        pose_timestamp_queue.push(msg.header.stamp.toSec());
        pos_queue.push(p0);
        att_queue.push(quad);

        /// Update yaw0 here, should be among [-PI, PI] 
        yaw0 = atan2(2*(quad.w()*quad.z()+quad.x()*quad.y()), 1-2*(quad.z()*quad.z()+quad.y()*quad.y()));

//        ROS_INFO("Current yaw = %f", yaw0);

        /*** velocity ***/
        /** NWU**/
        v0(0) = msg.twist.twist.linear.x;
        v0(1) = msg.twist.twist.linear.y;
        v0(2) = msg.twist.twist.linear.z;
        yaw0_rate = msg.twist.twist.angular.z;

        if(fabs(v0(0)) < 0.05) v0(0) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(1)) < 0.05) v0(1) = 0.0;  //add a dead zone for v0 used in motion primatives
        if(fabs(v0(2)) < 0.05) v0(2) = 0.0;  //add a dead zone for v0 used in motion primatives

        /** Calculate virtual accelerates from velocity. Original accelerates given by px4 is too noisy **/
        static bool init_v_flag = true;
        static float last_time, last_vx, last_vy, last_vz;
        
        if(init_v_flag){
            init_v_flag = false;
        }
        else{
            double delt_t = ros::Time::now().toSec() - last_time;
            a0(0) = (v0(0) - last_vx) / delt_t;
            a0(1) = (v0(1) - last_vy) / delt_t;
            a0(2) = (v0(2) - last_vz) / delt_t;

            if(fabs(a0(0)) < 0.2 || !isfinite(a0(0))) a0(0) = 0.0;  //dead zone for acc x
            if(fabs(a0(1)) < 0.2 || !isfinite(a0(1))) a0(1) = 0.0; //dead zone for acc y
            if(fabs(a0(2)) < 0.2 || !isfinite(a0(2))) a0(2) = 0.0; //dead zone for acc z
        }

        last_time = ros::Time::now().toSec();
        last_vx = v0(0);
        last_vy = v0(1);
        last_vz = v0(2);

        state_updating = false;
        
    }

    if(fabs(p0(1))>DIRECTION_CHANGE_LEFT_SIZE && (p0(1))*(p_goal(1)) > 0 && DIRECTION_AUTO_CHANGE){
        randomGoalGenerate();
    }
}


void uavModeCallback(const mavros_msgs::State &msg)
{
    if(msg.mode=="OFFBOARD") offboard_ready=true;
    else offboard_ready=false;
}


void sendMotorCommands(float yaw) // Range[-Pi, Pi], [0, 1]
{
    ///TODO: delete yaw = 0!!!!!
    yaw = 0.f;

    /** Set a limitation **/
    float delt_yaw = yaw - motor_yaw;
    if(fabs(delt_yaw) > head_max_yaw_delt){
        yaw = motor_yaw + head_max_yaw_delt * delt_yaw / fabs(delt_yaw);
    }

    if(!if_in_simulation){
        static geometry_msgs::Point32 head_cmd;
        head_cmd.x = -yaw + init_head_yaw; 
        head_cmd.y = motor_velocity_set;
        head_cmd_pub.publish(head_cmd);
    }else{
        static std_msgs::Float64 head_cmd;
        head_cmd.data = yaw - init_head_yaw;  
        head_cmd_pub.publish(head_cmd);
    }
    
}


void getParameterList(ros::NodeHandle nh){
    /*** Read parameter list ***/
    nh.getParam("/dynamic_map_planning/goal_position_x", p_goal(0));
    nh.getParam("/dynamic_map_planning/goal_position_y", p_goal(1));
    nh.getParam("/dynamic_map_planning/goal_position_z", p_goal(2));
    p_goal_raw = p_goal;

    nh.getParam("/dynamic_map_planning/MAX_V_XY", MAX_V_XY);
    nh.getParam("/dynamic_map_planning/MAX_V_Z_UP", MAX_V_Z_UP);
    nh.getParam("/dynamic_map_planning/MAX_V_Z_DOWN", MAX_V_Z_DOWN);
    nh.getParam("/dynamic_map_planning/MAX_A", MAX_A);
    nh.getParam("/dynamic_map_planning/distance_reference_length", pp.d_ref);
    nh.getParam("/dynamic_map_planning/toward_goal_k1_xy", pp.k1_xy);
    nh.getParam("/dynamic_map_planning/toward_goal_k1_z", pp.k1_z);
    nh.getParam("/dynamic_map_planning/less_variation_k2_xy", pp.k2_xy);
    nh.getParam("/dynamic_map_planning/less_variation_k2_z", pp.k2_z);
    nh.getParam("/dynamic_map_planning/v_max_at_goal_ori", pp.v_max_at_goal_ori);
    nh.getParam("/dynamic_map_planning/collision_threshold_static", pp.collision_threshold_static);
    nh.getParam("/dynamic_map_planning/collision_threshold_dynamic", pp.collision_threshold_dynamic);
    nh.getParam("/dynamic_map_planning/k_current_v", hp.k_current_v);
    nh.getParam("/dynamic_map_planning/k_planned_direction", hp.k_planned_direction);
    nh.getParam("/dynamic_map_planning/k_v_fluctuation", hp.k_v_fluctuation);
    nh.getParam("/dynamic_map_planning/k_dynamic_objects", hp.k_dynamic_objects);
    nh.getParam("/dynamic_map_planning/k_common_update", hp.k_common_update);
    nh.getParam("/dynamic_map_planning/if_publish_panel_arrays", if_publish_panel_arrays);
    nh.getParam("/dynamic_map_planning/CAMERA_H_FOV", CAMERA_H_FOV);
    nh.getParam("/dynamic_map_planning/kp_xy", pt.kp_xy);
    nh.getParam("/dynamic_map_planning/kp_z", pt.kp_z);
    nh.getParam("/dynamic_map_planning/kp_yaw", pt.kp_yaw);
    nh.getParam("/dynamic_map_planning/p_2_delt_v_max_xy", pt.p_2_delt_v_max_xy);
    nh.getParam("/dynamic_map_planning/p_2_delt_v_max_z", pt.p_2_delt_v_max_z);
    nh.getParam("/dynamic_map_planning/max_yaw_rate", pt.max_yaw_rate);
    nh.getParam("/dynamic_map_planning/head_max_yaw_delt", head_max_yaw_delt);
    nh.getParam("/dynamic_map_planning/motor_velocity_set", motor_velocity_set);
    nh.getParam("/dynamic_map_planning/if_plan_vertical_path", if_plan_vertical_path);
    nh.getParam("/dynamic_map_planning/if_in_simulation", if_in_simulation);
    nh.getParam("/dynamic_map_planning/if_flyback_enable", if_flyback_enable);
    nh.getParam("/dynamic_map_planning/HEIGHT_LIMIT", HEIGHT_LIMIT);
    nh.getParam("/dynamic_map_planning/XY_LIMIT", XY_LIMIT);
    nh.getParam("/dynamic_map_planning/PLAN_INIT_STATE_CHANGE_THRESHOLD", PLAN_INIT_STATE_CHANGE_THRESHOLD);
    nh.getParam("/dynamic_map_planning/DIRECTION_CHANGE_LEFT_SIZE", DIRECTION_CHANGE_LEFT_SIZE);
    nh.getParam("/dynamic_map_planning/DIRECTION_AUTO_CHANGE", DIRECTION_AUTO_CHANGE);
    nh.getParam("/dynamic_map_planning/MAP_DELAY_SECONDS", MAP_DELAY_SECONDS);

    if(if_in_simulation)  ROS_WARN("In simulation mode");

    ROS_INFO("Parameters list reading finished! Goal position is: (%f, %f, %f), MAX Vel is %f", p_goal(0), p_goal(1), p_goal(2), MAX_V_XY);
}


int main(int argc, char** argv)
{
    ros::init(argc, argv, "local_planning_rank_with_future_risk");
    ros::NodeHandle nh;

    setParameters();    /// chg, use parameters defined here
    getParameterList(nh);

    trajectory_time_table->csv2pva_table("/home/clarence/ros_ws/map_ws/src/tables/p3-2_v3_a4_res0-1.csv");
    table_initialized = true;
    ROS_WARN("trajectory_time_table loaded!");


    my_map.setPredictionVariance(0.05, 0.1);  //0.1, 0.1
    my_map.setObservationStdDev(0.01);
//    my_map.setParticleRecordFlag(1, 22.5);
    my_map.setNewBornParticleNumberofEachPoint(30);
    my_map.setNewBornParticleWeight(0.01);
    my_map.setOriginalVoxelFilterResolution(res);


    // State parameters initiate
    global_time_now = ros::Time::now().toSec();

    p0 << 0.0, 0.0, 0.0;
    v0 << 0.0, 0.0, 0.0;
    a0 << 0.0, 0.0, 0.0;
//    p_goal << -1.0, -5.0, 1.2;
    yaw0 = 0.0;

    // Fov sample parameters
    Fov_half << 35, 20;
    // Horizontal angles larger than 90 degree are deleted
    //Angle_h << -90, -70, -50, -30, -20, -10, 0, 10, 20, 30, 50, 70, 90;
    if(if_flyback_enable){
//        Angle_h << -130, -90, -75, -60, -45, -30, -25, -20, -15, -10, -5, 0, 5, 10, 15, 20, 25, 30, 45, 60, 75, 90, 130;
        Angle_h << -120, -112, -104, -96, -88, -80, -72, -64, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 64, 72, 80, 88, 96, 104, 112, 120;
    }else{
//        Angle_h << -90, -80, -70, -60, -50, -40, -30, -20, -10, 0, 5, 10, 20, 30, 40, 50, 60, 70, 80, 90;
        Angle_h << -60, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 60;

    }

    if(if_plan_vertical_path){
        // Angle_v << -60, -56, -48, -40, -32, -24, -16, -8, 0, 8, 16, 24, 32, 40, 48, 56, 60;
        Angle_v << -24, -16, -8, 0, 8, 16, 24;
    } else{
        Angle_v << 0, 0, 0, 0, 0, 0, 0;
    }

    if(Angle_h.size() != ANGLE_H_NUM || Angle_v.size() != ANGLE_V_NUM){
        ROS_INFO(" ANGLE_H_NUM or ANGLE_V_NUM mismatches the initialized vector. Please check!");
        return 0;
    }

    Angle_h = Angle_h * M_PI / 180.0;
    Angle_v = Angle_v * M_PI / 180.0;
    Fov_half = Fov_half * M_PI / 180.0;

    _direction_update_buffer = Eigen::VectorXf::Zero(LOOKING_PIECES_SIZE); 
    heading_resolution =  2.f * PI / (float)LOOKING_PIECES_SIZE;
    mid_seq_num = (int)(LOOKING_PIECES_SIZE / 2); // start from 0, mid is 9 when LOOKING_PIECES_SIZE is 18

    int valid_piece_num = (int)((float)CAMERA_H_FOV / 180.f * PI / heading_resolution);
    if(valid_piece_num % 2 == 0)  valid_piece_num -= 1;
    if(valid_piece_num < 1){
        ROS_ERROR("No enough view field with the current camera set!");
        return 0;
    }
    valid_piece_num_one_side = (valid_piece_num - 1) / 2;

    ROS_INFO("Heading resolution = %f (rad), Fov = %f, valid_piece_num = %d", heading_resolution, CAMERA_H_FOV, valid_piece_num);

    send_buffer_size_max = 1.f / SEND_DURATION;
    const int const_send_buffer_size_max = send_buffer_size_max;
    send_traj_buffer_p = new Eigen::Vector3d[const_send_buffer_size_max];
    send_traj_buffer_v = new Eigen::Vector3d[const_send_buffer_size_max];
    send_traj_buffer_a = new Eigen::Vector3d[const_send_buffer_size_max];

    // Random seed
    std::srand((unsigned)time(NULL));

    // ringbuffer cloud2
    future_status_pub = nh.advertise<sensor_msgs::PointCloud2>("/my_map/future_status", 1, true);
    cloud_pub = nh.advertise<sensor_msgs::PointCloud2>("/my_map/cloud_ob", 1, true);
    map_center_pub = nh.advertise<geometry_msgs::PoseStamped>("/map_center",1,true) ;

    goal_pub = nh.advertise<geometry_msgs::PointStamped>("/goal_position",1,true);
    current_marker_pub = nh.advertise<visualization_msgs::Marker>("/visualization_marker", 1);

    motor_pub = nh.advertise<geometry_msgs::Point32>("/place_velocity_info_corrected", 1, true); 

    ros::Subscriber mode_sub, position_isolate_sub, velocity_isolate_sub, motor_sub, cloud_sub, sim_odom_isolate_sub;
    pva_pub = nh.advertise<trajectory_msgs::JointTrajectoryPoint>("/pva_setpoint", 1, true);

    /*** For real world test***/
    if(!if_in_simulation){
        mode_sub = nh.subscribe("/mavros/state", 1, uavModeCallback);
        position_isolate_sub =  nh.subscribe("/mavros/local_position/pose", 1, positionCallback);
        velocity_isolate_sub = nh.subscribe("/mavros/local_position/velocity_local", 1, velocityCallback);

        // typedef message_filters::sync_policies::ApproximateTime<control_msgs::JointControllerState,sensor_msgs::PointCloud2> mapSyncPolicy;
        // message_filters::Subscriber<control_msgs::JointControllerState> *motor_sub;
        // message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_sub;
        // motor_sub = new message_filters::Subscriber<control_msgs::JointControllerState>(nh, "/place_velocity_info_joint_state", 2);
        // cloud_sub = new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/d400/depth/color/points", 2);

        // message_filters::Synchronizer<mapSyncPolicy>* sync_;
        // sync_ = new message_filters::Synchronizer<mapSyncPolicy>(mapSyncPolicy(10), *motor_sub, *cloud_sub);
        // sync_->registerCallback(boost::bind(&cloudCallback, _1, _2));

        cloud_sub = nh.subscribe("/camera_front/depth/points", 1, cloudCallback);

        head_cmd_pub = nh.advertise<geometry_msgs::Point32>("/gimbal_commands", 2, true); 
    }else{
         /*** For simulation test***/
        offboard_ready = true;

        // sim_odom_isolate_sub = nh.subscribe("/mavros/local_position/odom", 1, simPositionVelocityCallback);

        position_isolate_sub =  nh.subscribe("/mavros/local_position/pose", 1, positionCallback);
        velocity_isolate_sub = nh.subscribe("/mavros/local_position/velocity_local", 1, velocityCallback);

        // typedef message_filters::sync_policies::ApproximateTime<control_msgs::JointControllerState,sensor_msgs::PointCloud2> mapSyncPolicy;
        // message_filters::Subscriber<control_msgs::JointControllerState> *motor_sub;
        // message_filters::Subscriber<sensor_msgs::PointCloud2> *cloud_sub;
        // motor_sub = new message_filters::Subscriber<control_msgs::JointControllerState>(nh, "/iris/joint1_position_controller/state", 2);
        // cloud_sub = new message_filters::Subscriber<sensor_msgs::PointCloud2>(nh, "/iris/vi_sensor/camera_depth/depth/points", 2);

        // message_filters::Synchronizer<mapSyncPolicy>* sync_;
        // sync_ = new message_filters::Synchronizer<mapSyncPolicy>(mapSyncPolicy(10), *motor_sub, *cloud_sub);
        // sync_->registerCallback(boost::bind(&cloudCallback, _1, _2));

        cloud_sub = nh.subscribe("/camera_front/depth/points", 1, cloudCallback);

        head_cmd_pub = nh.advertise<std_msgs::Float64>("/iris/joint1_position_controller/command", 2, true);
        if(if_sim_lee_position_controller){
            sim_trajectory_pub = nh.advertise<geometry_msgs::Pose>("/trajectory_setpoint", 2, true);
        }

        if(DIRECTION_AUTO_CHANGE){
            randomGoalGenerate();
        }
        
    }

    // timer for publish ringbuffer as pointcloud
//    ros::Timer timer1 = nh.createTimer(ros::Duration(0.2), cloudPubCallback); // RATE 5 Hz to publish

    // timer for trajectory generation
    /// ***** don't excute this callback in data collection, chg
    ros::Timer timer2 = nh.createTimer(ros::Duration(CAL_DURATION), trajectoryCallback);

    /***** Publisher for visulization ****/
    cost_head_update_degree_pub = nh.advertise<std_msgs::Float64MultiArray>("/head_cost/cost_head_update",1,true);
    cost_head_velocity_pub = nh.advertise<std_msgs::Float64MultiArray>("/head_cost/cost_head_velocity",1,true);
    cost_head_direction_pub = nh.advertise<std_msgs::Float64MultiArray>("/head_cost/cost_head_direction",1,true);
    cost_head_objects_pub = nh.advertise<std_msgs::Float64MultiArray>("/head_cost/cost_head_objects",1,true);
    cost_head_fluctuation_pub = nh.advertise<std_msgs::Float64MultiArray>("/head_cost/cost_head_fluctuation",1,true);
    cost_head_final_pub = nh.advertise<std_msgs::Float64MultiArray>("/head_cost/cost_head_final",1,true);

    std::cout << "Start mapping!" << std::endl;

    // ros::spin();
    ros::AsyncSpinner spinner(3); // Use 3 threads
    spinner.start();
    ros::waitForShutdown();

    return 0;
}



void setParameters(){
    p_goal(0) = 60.0;
    p_goal(1)= 0.0;
    p_goal(2)=1.2;
    p_goal_raw = p_goal;

    MAX_V_XY = 3;
    MAX_V_Z_UP = 0.8;
    MAX_V_Z_DOWN = 0.4;
    MAX_A = 3;
    pp.buffer_send_counter = 1;  // To eliminate the influence of delay
    pp.d_ref = 2.2;
    pp.k1_xy = 4.0;
    pp.k1_z = 4.0;
    pp.k2_xy = 1.0;
    pp.k2_z = 1.0;
    pp.v_max_at_goal_ori = 0.3;
    pp.collision_threshold_static = 0.5;
    pp.collision_threshold_dynamic = 0.5;
    hp.k_current_v = 0.7;
    hp.k_planned_direction = 0.2;
    hp.k_v_fluctuation = 0.4;
    hp.k_dynamic_objects = 1.0;
    hp.k_common_update = 0.1;
    if_publish_panel_arrays = true;
    CAMERA_H_FOV = 62;
    pt.kp_xy = 0.18;
    pt.kp_z = 0.13;
    pt.kp_yaw = 0.05;
    pt.p_2_delt_v_max_xy = 0.7;
    pt.p_2_delt_v_max_z = 0.4;
    pt.max_yaw_rate = 1.0;
    head_max_yaw_delt = 0.5;
    motor_velocity_set = 40;
    if_plan_vertical_path = false;  
    if_in_simulation = true;
    if_flyback_enable = false;
    HEIGHT_LIMIT = 2.8;
    XY_LIMIT = 2.2;
    PLAN_INIT_STATE_CHANGE_THRESHOLD = 0.1;
    DIRECTION_CHANGE_LEFT_SIZE = 1.5;
    DIRECTION_AUTO_CHANGE = false;
    MAP_DELAY_SECONDS = 0.05;
}


// Created by clarence on 19-8-29.
//

